/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
 * \file
 * \brief Ceilbot :)
 * \author Antti Maula <antti.maula@tkk.fi>
 */

#include <libxml/parser.h>
#include <libxml/tree.h>
#include "Ceilbot.h"
#include "drawutils.h"
#include "World.h"
#include "owndebug.h"

//*****************************************************************************
//*****************************************************************************
static float MAX_MOTOR_SPEED = 10.0;
static float MAX_ANGULAR_SPEED = 10.0;
static float MAX_ACCELERATION = 10.0;
static float POSITION_INTERVAL = 0.05;
static float LASER_INTERVAL = 0.1;
//*****************************************************************************

CObject* CCeilbot::LoadMLMA(CWorld *world, xmlNode *node)
{
  CObject *object=NULL;

  // Parameters for a Line.
  xmlChar *xmlX = xmlGetProp(node, (xmlChar *) "x");
  xmlChar *xmlY = xmlGetProp(node, (xmlChar *) "y");
  xmlChar *xmlName = xmlGetProp(node, (xmlChar *) "name");
  xmlChar *xmlGroup = xmlGetProp(node, (xmlChar *) "MaCIGroup");

  xmlChar *xmlWidth = xmlGetProp(node, (xmlChar *) "width");
  xmlChar *xmlHeight = xmlGetProp(node, (xmlChar *) "height");
  xmlChar *xmlLength = xmlGetProp(node, (xmlChar *) "length");
  

  // Process.
  if (xmlX && xmlY && (xmlName || xmlGroup)) {


    // Convert values.
    float x,y;
    float w=0.99, h=0.99, l=0.99;
    std::string name = xmlName ? (const char*)xmlName : "";
    std::string group = xmlGroup ? (const char*)xmlGroup : "";;
    x = atof((const char*)xmlX);
    y = atof((const char*)xmlY);
    if (xmlWidth) w = atof((const char*)xmlWidth);
    if (xmlHeight) h = atof((const char*)xmlHeight);
    if (xmlLength) l = atof((const char*)xmlLength);

    // construct object.
    object=new CCeilbot(world, name, group,
                        x, y, w, h, l);

  } else {
    dPrint(1,"MLMA Error:Not enough parameters defined for MottiPbV-object!");

  }

  // Cleanup (Always do this to clean out properties which were found)
  if (xmlX) xmlFree(xmlX);
  if (xmlY) xmlFree(xmlY);
  if (xmlName) xmlFree(xmlName);
  if (xmlGroup) xmlFree(xmlGroup);
  if (xmlWidth) xmlFree(xmlWidth);
  if (xmlHeight) xmlFree(xmlHeight);
  if (xmlLength) xmlFree(xmlLength);
  
  // Finally; insert the object to world.
  if (object) {
    world->InsertObject(object);
  }

  // And return it too.
  return object;
}
//*****************************************************************************

CCeilbot::CCeilbot(CWorld* World,
                   const std::string &aGIMnetName,
                   const std::string &aMaCIGroup,
                   const float &x, const float &y, 
                   const float &w, const float &h, const float &l)
  : CObject(World,NULL),
    gimi(),
    mcs(&gimi),
    SpeedCtrlServer(&mcs,0),
    PositionServer(&mcs,0),
    EnergyServer(&mcs,0),
    CoordinateDriveServer(&mcs,0),
    gimnetName(aGIMnetName),
    MaCIGroup(World->GetBaseMaCIGroup()+"."+aMaCIGroup),
    speed(0), angularSpeed(0), posX(x), posY(-y),
    heading((heading-90.0)*M_PI/180.0),
    positionTimer(0), energyTimer(0),
    iCoordinates(0),
    iDrive(0),
    chassis(NULL),
    LaserScanner(NULL)
{
  memset(&chassisMaterial, 0x00, sizeof(chassisMaterial));
  chassisMaterial.look.color[0]=0.0;
  chassisMaterial.look.color[1]=0.3;
  chassisMaterial.look.color[2]=0.9;
  chassisMaterial.density = 10.0;
  
  Parts.push_back((chassis = new CBox(w, h, l,
                                      this, this->SpaceID, 0, &chassisMaterial, false)));
  
  // Make collideable :)
  //chassis->NotifyCollisions=true;
  
  // Aling chassis to world
  dBodySetPosition(chassis->BodyID, 
                   x, 
                   (h/2.0),
                   -y);

  //Set damping
  //dBodySetDamping (chassis->BodyID, 0.1, 0.1);


  dMatrix3 R;
  dRFromAxisAndAngle(R,0,1,0, 0.00); // Zero angle.
  dBodySetRotation(chassis->BodyID,R);
  
  
  //dReal pos[3]={0.00, -((h/2)-0.2), -(l/2)-0.01};
  dReal pos[3]={0.00, (h/2.00)+0.090, 0};
  dMatrix3 rot;
  dRFrom2Axes(rot, -1,0,0, 0,1,0);
  Children.push_back(LaserScanner = new CSick(World, 
                                              this,
                                              chassis->BodyID,
                                              pos,
                                              rot,
                                              LASER_INTERVAL,
                                              &mcs,
                                              0,
                                              "Laser"));
  LaserScanner->SetDevicePosition(-pos[2], // LENGTH AXIS
                                  0.00,    // WIDTH AXIS
                                  pos[1],  // HEIGHT AXIS
                                  0);
  
}
//*****************************************************************************

void CCeilbot::Activate()
{
  if (MaCIGroup.length()) {
    mcs.SetGroupName(MaCIGroup);

  } else {
    mcs.SetGroupName(gimnetName);

  }
  
  if (gimnetName.empty()) {
    gimnetName=MaCI::GetStaticClientName(MaCIGroup+"FSRSimCeilbot");
  }
  
  //SpeedCtrlServer.SetInterfaceInstanceName("Motion");
  PositionServer.SetInterfaceInstanceName("Motion");
  EnergyServer.SetInterfaceInstanceName("Energy");
  CoordinateDriveServer.SetInterfaceInstanceName("Motion");

  
  connectGIMI();
  MaCI::EMaCIError e;

  /*  e=SpeedCtrlServer.Open();
  if (e!=MaCI::KMaCIOK)
    {
      dPrint(1,"Could not open SpeedCtrlServer");
      abort();
    } else {
      SpeedCtrlServer.SetSpeedLimits(MAX_MOTOR_SPEED, MAX_ANGULAR_SPEED, MAX_ACCELERATION,
                                     -MAX_MOTOR_SPEED, -MAX_ANGULAR_SPEED, -MAX_ACCELERATION);
    }
  */

  e=PositionServer.Open();
  if (e!=MaCI::KMaCIOK) {
    dPrint(1,"Could not open PositionServer");
    abort();
    
  }
  
  e=EnergyServer.Open();
  if (e!=MaCI::KMaCIOK) {
    dPrint(1,"Could not open EnergyServer");
    abort();

  }
  

  // We activate the coordinatedriveserver
  e=CoordinateDriveServer.Open();
  if (e!=MaCI::KMaCIOK) {
    dPrint(1,"Could not open CoordinateDriveServer");
    abort();
    
  }


  for (ObjectsIter i=Children.begin();i!=Children.end();i++) {
    (*i)->Activate();
  }
 
  speed=0;
  angularSpeed=0;
  dBodySetLinearVel(chassis->BodyID,0,0,0);
  dBodySetAngularVel(chassis->BodyID,0,0,0);
}
//*****************************************************************************

void CCeilbot::GenerateEnergyEvent(void)
{
  static double last_update = 0;
  static const double running_begin = ownTime_get_s();

  if (ownTime_get_s_since(last_update) > 5.000) {
    const double charge_joules = 5 * 60; // Joules
    const double current_power = 1.00; // Watts
    const double used_joules = ownTime_get_s_since(running_begin) * current_power;
    
    double charge_percentage = 0.00;
    
    if (used_joules < charge_joules) {
      charge_percentage = 1.0 - (used_joules / charge_joules);
    }
    
    //dPrint(ODTEST,"Used joules = %f, percentage: %f", used_joules, charge_percentage);

    /// Set Energy state
    MaCI::Energy::CEnergyData ed;
    ed.CreateInternalBinBag();
    ed.SetTimestamp(MaCI::Common::TTimestamp());
    ed.SetEnergyEvent(MaCI::Energy::TEnergyEvent(MaCI::Energy::KEventTypeEnergyData, 
                                                 charge_percentage*100.0, 
                                                 charge_joules, 
                                                 current_power));
    
    // Broadcast!
    EnergyServer.SendEnergyEvent(ed);
    
    // Update
    last_update = ownTime_get_s();
  }
}
//*****************************************************************************

void CCeilbot::CoordDriver()
{
  if(iDrive == 0)
    return;



  // We stop the movement if the iCoordinates vector is empty
  if(iCoordinates.size() == 0)
    {
      iDrive = 0;
      
      // The simulator does not like absolute zero?
      int movespeed = 0.00001;
      dBodySetLinearVel(chassis->BodyID,movespeed*cos(heading),0,movespeed*sin(heading));
      dBodySetAngularVel(chassis->BodyID,0,0,0);

      return;

    }



  using namespace MaCI::CoordinateDrive;

  std::vector<TCoordinate2D>::iterator ite = iCoordinates.begin();
  float xpos = ite->x;
  float ypos = ite->y;


  // We use negative atan2 so we get the same system as FSR heading
  // Also we need to invert the robot's y position
  float targetHeading = -atan2((ypos - -posY), (xpos - posX) );
  

  
  // We need the heading difference between the robot heading and targetHeading
  float headingDifference = targetHeading - heading;

  // We need to take into account the jump from -pi to +pi.
  // If the headingDifference is over pi (heading -3, target +3 -> diff = 6),
  // then we take 2*pi away from the difference. If the heading difference is under pi
  // then we add 2*pi to the difference.

  if(headingDifference >= M_PI)
    {
      headingDifference -= 2*M_PI;
    }
  else if(headingDifference < -M_PI)
    {
      headingDifference += 2*M_PI;
    }
  

  /**
  printf("FSR location:  x:%lf  y:%lf\n", posX, posY);
  printf("Target location: x:%lf y:%lf\n", xpos, ypos);
  // printf("Dx: %lf Dy: %lf\n", xpos - posX, ypos - posY);
  printf("FSR heading is: %lf\n", heading);
  printf("Target heading: %lf\n", targetHeading);
  printf("Heading difference: %lf\n\n\n", headingDifference);
  */


  
  // We stop the robot if we have reached the waypoint
    if(fabs(xpos - posX) < 0.1 && fabs(ypos - -posY) < 0.1)
    {

      // We stop the robot
      int movespeed = 0.00001;
      dBodySetLinearVel(chassis->BodyID,movespeed*cos(heading),0,movespeed*sin(heading));
      dBodySetAngularVel(chassis->BodyID,0,0,0);

      // printf("WAYPOINT REACHED\n");

      // printf("Sending arrivedToPointEvent\n");
      // We send the ArrivedToPointEvent
      CCoordinateDriveData replyData;
      gim::binbag::CBinBag bb;
      replyData.EncodeTo(&bb);
      replyData.SetCommand(TCommand(KCommandArrivedToPointEvent));
      replyData.SetCoordinate2D(iCoordinates[0]);
      //replyData.Print(1);
      CoordinateDriveServer.SendArrivedToTargetEvent(replyData);
      // printf("ArrivedToPointEvent sent\n");
          

      // We remove the front coordinates 
      iCoordinates.erase(iCoordinates.begin());



      // printf("Sending coords\n");
      //SEnd event of coordinates
      CCoordinateDriveData data;
      gim::binbag::CBinBag bb2;
      const TCoordinateHeader header(iCoordinates.size());
      data.EncodeTo(&bb2);

      data.SetCommand(TCommand(KCommandCoordinatesEvent));
      data.SetCoordinateHeader(header);


      for (unsigned int i = 0; i< iCoordinates.size();i++){
        data.SetCoordinate2D(iCoordinates[i]);
          }

      data.SetTimestamp(MaCI::Common::TTimestamp());     
      CoordinateDriveServer.SendCoordinatesEvent(data);
      // printf("Coords sent!\n");

      //sleep(1);
      // We return
      return;



    }



  // We turn towards the correct heading
  dBodySetAngularVel(chassis->BodyID,0,-(headingDifference)/0.02,0);


  // We calculate the distance between this waypoint and the next
  float dist = sqrt ( (xpos - posX)*(xpos - posX) + (ypos - -posY)*(ypos - -posY));

  // printf("Distance is %lf\n", dist);



  // We move towards the new position only if the heading is good enough
  if(fabs(headingDifference) < 0.02)
    {
      int movespeed = dist/0.02;
      dBodySetLinearVel(chassis->BodyID,movespeed*cos(heading),0,movespeed*sin(heading));
    } else
      {
        int movespeed = 0.001;
        dBodySetLinearVel(chassis->BodyID,movespeed*cos(heading),0,movespeed*sin(heading));

      }


  return;
  //  dBodySetLinearVel(chassis->BodyID,speed*cos(heading),0,speed*sin(heading));
  //dBodySetAngularVel(chassis->BodyID,0,angularSpeed,0);
}
//*****************************************************************************

void CCeilbot::Update(float StepSize)
{
  

  // printf("StepSize is: %lf\n",StepSize);

  const dReal *P, *R;
  dReal v[3]={0,0,1};
  dReal r[3];
  float h;
  P=dBodyGetPosition(chassis->BodyID);
  R=dBodyGetRotation(chassis->BodyID);
  RotateVector(R,v,r);
  //#warning "NOTE: Using inverted y-axis and heading"
  h=atan2(-r[2],-r[0]);
  
  posX=P[0];
  posY=P[2];
  heading=h;

  gim::binbag::CBinBag bag;

  speed=10.0;
  angularSpeed=10.0;
          

  /*
  MaCI::SpeedCtrl::CSpeedCtrlData d;
  if (SpeedCtrlServer.WaitCommand(d,0,true)==MaCI::KMaCIOK) {
    const MaCI::SpeedCtrl::TCommand *c=d.GetCommand();
    const MaCI::SpeedCtrl::TSpeed *s;
    if (c) {
      switch (c->cmd) {
      
      case MaCI::SpeedCtrl::KCommandSetSpeed:
        s=d.GetSpeed();
        if (s) {
          //          dPrint(ODTEST,"Set Speed: %f, Angular: %f", s->speed, s->angularSpeed);
          speed=s->speed;
          angularSpeed=s->angularSpeed;
          dBodySetLinearVel(chassis->BodyID,speed*cos(heading),0,speed*sin(heading));
          dBodySetAngularVel(chassis->BodyID,0,angularSpeed,0);
          
        }
        break;
      
      case MaCI::SpeedCtrl::KCommandSetStop:
        speed=0;
        angularSpeed=0;
        //s->acceleration;
        dPrint(1,"Set speed");
        //dBodySetLinearVel(chassis->BodyID,0,0,speed);
        dBodySetLinearVel(chassis->BodyID,0,0,0);
        dBodySetAngularVel(chassis->BodyID,0,0,0);
        break;
      
      case MaCI::SpeedCtrl::KCommandTimeout:
        speed=0;
        angularSpeed=0;
        dBodySetLinearVel(chassis->BodyID,0,0,0);
        dBodySetAngularVel(chassis->BodyID,0,0,0);
        break;
      
      }
    }
  }

  MaCI::SpeedCtrl::CSpeedCtrlData speedEvent;
  speedEvent.EncodeTo(&bag); 
  speedEvent.SetCommand(MaCI::SpeedCtrl::TCommand(MaCI::SpeedCtrl::KEventSpeed));
  speedEvent.SetSpeed(MaCI::SpeedCtrl::TSpeed(speed,angularSpeed));
  speedEvent.SetTimestamp(MaCI::Common::TTimestamp());
  SpeedCtrlServer.SendSpeedEvent(speedEvent);
  */

  if (positionTimer>=POSITION_INTERVAL)
    {
      MaCI::Position::CPositionData pd;
      bag.Clear();
      pd.EncodeTo(&bag);
      pd.SetTimestamp(MaCI::Common::TTimestamp());
      MaCI::Position::TPose2D pose(posX,-posY,-heading);
      pose.NormalizeAngle();

      pd.SetPose2D(pose);
      pd.SetVariance2D(MaCI::Position::TVariance2D(0,0,0));
      pd.SetProbability(MaCI::Position::TProbability(1));

      PositionServer.SendPositionEvent(pd);

      positionTimer=0;
    }
  
  positionTimer+=StepSize;
  ////////////////////////////////////////////////////////////////////////////
  //Doing coordinatedriveserver stuff here
  

  MaCI::EMaCIError e;

  using namespace MaCI::CoordinateDrive;

  // Construct Data element
  MaCI::CoordinateDrive::CCoordinateDriveData cdData;
  gim::binbag::CBinBag bb;
  MaCI::CoordinateDrive::TReplyInfo rInfo(0,0);
  // Now, sit back and wait for command to arrive (for 1000ms)
  e = CoordinateDriveServer.WaitCommand(cdData, rInfo,0);

  // Got command? (No timeout or error)
  if (e == MaCI::KMaCIOK) {
    //dPrint(1,"Got command");
    const  MaCI::CoordinateDrive::TCommand *cmd = cdData.GetCommand();

    // cdData.Print(1);
    assert(cmd != NULL);
    // Switch based on command type
    switch(cmd->cmd) {
    case MaCI::CoordinateDrive::KCommandSetCoordinate2D:{
      const MaCI::CoordinateDrive::TCoordinate2D *coord = cdData.GetCoordinate(0);
      if(coord){

        
        dPrint(1,"Got Command - SetCoordinate2D = (%f,%f,%f), (%d,%d,%d)",
               coord->x,
               coord->y,
               coord->a,
               coord->isDifferential,
               coord->isPathTarget,
               coord->isWaitPoint);
        

        iCoordinates.push_back(*coord);

        
        
        //SEnd event of coordinates
        MaCI::CoordinateDrive::CCoordinateDriveData data;
        gim::binbag::CBinBag bb2;
        const TCoordinateHeader header(iCoordinates.size());
        data.EncodeTo(&bb2);
        data.SetCommand(TCommand(KCommandCoordinatesEvent));
        data.SetTimestamp(MaCI::Common::TTimestamp());
        data.SetCoordinateHeader(header);
        for (unsigned int i = 0; i< iCoordinates.size();i++){
          data.SetCoordinate2D(iCoordinates[i]);
        }
        if(CoordinateDriveServer.SendCoordinatesEvent(data)){
          // dPrint(1,"Succesfully sent coordinateDrive point event");
        }else{
          // dPrint(1,"Failed to send coordinateDrive point event");
        }


      }else{
        // dPrint(1,"Coord == NULL");
      }
	    
      break;
    }

    case KCommandSetStart:{

      // printf("STARTING MOVE\n");
      iDrive = 1;

      // dPrint(1,"Got Command - Set Start");
      break;
    }

    case KCommandSetStop:{

      iDrive = 0;
      // printf("STOPPING MOVE\n");

      // dPrint(1,"Got Command - Set Stop");
      break;
    }
 
      case KCommandRemoveCoordinates:{
        // dPrint(1,"Got Command - Remove Coordinates");
        iCoordinates.clear();
        CCoordinateDriveData data;
        gim::binbag::CBinBag bb2;
        const TCoordinateHeader header(0);
        data.EncodeTo(&bb2);
        data.SetCommand(TCommand(KCommandCoordinatesEvent));
        data.SetTimestamp(MaCI::Common::TTimestamp());
        data.SetCoordinateHeader(header);
        CoordinateDriveServer.SendCoordinatesEvent(data);
        //Remove all the normal target points
        
        break;
      }
        
        


    default:{
      dPrint(1,"No implementation for command %d!", cmd->cmd);
      break;
    }
    }   
        
  } else if (e == MaCI::KMaCITimeout) {
    // No, timeout waiting for command. Print and wait again.
    dPrint(8,"Timeout waiting for Command");
  
  } else {
    // Quck? Some error occured? Print it, sleep an extra second and try again.
    dPrint(1,"Failed to WaitCommand()! (%s)",
           GetErrorStr(e).c_str());
    ownSleep_ms(1000);
  }



  // We start the CoordDriver
  CoordDriver();



  // Coordinatedriveserver stuff ends here
  ///////////////////////////////////////////////////////////////////



  // Energy
  GenerateEnergyEvent();

  

  for (ObjectsIter i=Children.begin();i!=Children.end();i++)
    {
      (*i)->Update(StepSize);
    }
  
}
//*****************************************************************************

void CCeilbot::OnCollision(CPrimitive *primitive, int numc, dContact *contacts, CPrimitive *primitive2)
{
}
//*****************************************************************************

void CCeilbot::Draw()
{
  for (ObjectsIter i=Children.begin();i!=Children.end();i++) {
    (*i)->Draw();
  }
  
  for (PrimitivesIter i=Parts.begin();i!=Parts.end();i++){
    (*i)->Draw();
  }
  
  
  dReal M[16];
  
  glMatrixMode(GL_MODELVIEW);
  glPushMatrix();
  
  //glLoadIdentity();
  
  chassis->GetGeomTransMatrix(M);
  glMultMatrixf((const GLfloat*)M);
  
  //   glTranslatef(posX, 0, posY);
  //   glRotatef(heading*180.0/M_PI+90,0,1,0);
  
  /*  glBegin(GL_LINES);
  //x
  glColor3f(1,0,0);
  glVertex3f(0,0,0);
  glVertex3f(1,0,0);
  //z
  glColor3f(0,0,1);
  glVertex3f(0,0,0);
  glVertex3f(0,1,0);
  //y
  glColor3f(0,1,0);
  glVertex3f(0,0,0);
  glVertex3f(0,0,-1);
  glEnd();*/
  
  /*
    glColor3f(1.0,0.3,0.1);
    drawBox( CHASSIS_WIDTH, CHASSIS_HEIGHT, CHASSIS_LENGTH, 0, CHASSIS_HEIGHT/2.0 + GROUND_CLEARANCE, 0);
  
    glColor3f(0.1,0.1,0.1);
    drawBox( FORK_WIDTH, FORK_HEIGHT, FORK_LENGTH, -FORK_GAP/2.0, 
    FORK_HEIGHT/2.0 + GROUND_CLEARANCE, FORK_LENGTH/2.0+CHASSIS_LENGTH/2.0);
  
    drawBox( FORK_WIDTH, FORK_HEIGHT, FORK_LENGTH, FORK_GAP/2.0, 
    FORK_HEIGHT/2.0 + GROUND_CLEARANCE, FORK_LENGTH/2.0+CHASSIS_LENGTH/2.0);
  */
  glPopMatrix();
  
}
//*****************************************************************************

void CCeilbot::connectGIMI()
{
  if (!SimWorld->ConnectGimi(&gimi,gimnetName)) {
    dPrint(ODTEST, "Failed to Connect to GIMI! Fatal error!");
    abort();
    
  }
}
//*****************************************************************************
//*****************************************************************************

